#include "MapPoint.h"


using namespace std;
using namespace cv;


mutex MapPoint::g_GlobalMutex;
static long unsigned int  g_mapPointCount = 0;

MapPoint::~MapPoint()
{


}


MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap):
	m_FirstKeyFrameId(pRefKF->m_KeyFrameId),
	m_FirstFrameId(pRefKF->m_frameId),
	m_ObsNumber(0),
	m_TrackReferenceForFrameId(0),
	m_LastFrameSeenId(0),
	m_BALocalForKeyFrameId(0),
	m_FuseCandidateForKeyFrameId(0),
	m_RefKF(pRefKF),
	m_VisibleCount(1),
	m_FoundCount(1),
	m_BadMapPoint(false),
	m_Replaced(static_cast<MapPoint*>(NULL)),
	m_Map(pMap),
	m_scaleInvDistance(0)
{
	 Pos.copyTo(m_WorldPos);
	 m_NormalVector = cv::Mat::zeros(3,1,CV_32F);

	 // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
	 unique_lock<mutex> lock(m_Map->m_MutexPointCreation);
	 m_MapPointId = g_mapPointCount++;
}

MapPoint::MapPoint(const cv::Mat &Pos,  Map* pMap, Frame* pFrame, const int &idxF):
	m_FirstKeyFrameId(-1),
	m_FirstFrameId(pFrame->m_frameId),
	m_ObsNumber(0),
	m_TrackReferenceForFrameId(0),
	m_LastFrameSeenId(0),
	m_BALocalForKeyFrameId(0),
	m_FuseCandidateForKeyFrameId(0),
	m_RefKF(static_cast<KeyFrame*>(NULL)),
	m_VisibleCount(1),
	m_FoundCount(1),
	m_BadMapPoint(false),
	m_Replaced(static_cast<MapPoint*>(NULL)),
	m_Map(pMap)
{
	Pos.copyTo(m_WorldPos);
	cv::Mat Ow = pFrame->GetCameraCenter();
	m_NormalVector = m_WorldPos - Ow;
	m_NormalVector = m_NormalVector/cv::norm(m_NormalVector);

	pFrame->m_DescriptorsLeft.row(idxF).copyTo(m_MapPointDescriptor);

	cv::Mat PC = Pos - Ow;

	m_scaleInvDistance = cv::norm(PC);

	// MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
	unique_lock<mutex> lock(m_Map->m_MutexPointCreation);
	m_MapPointId = g_mapPointCount++;
}



void MapPoint::SetWorldPos(const cv::Mat &Pos)
{
	unique_lock<mutex> lock2(g_GlobalMutex);
	unique_lock<mutex> lock(m_MutexPos);
	Pos.copyTo(m_WorldPos);
}

cv::Mat MapPoint::GetWorldPos()
{
	unique_lock<mutex> lock(m_MutexPos);
	return m_WorldPos.clone();
}


cv::Mat MapPoint::GetNormal()
{
	unique_lock<mutex> lock(m_MutexPos);
	return m_NormalVector.clone();
}


KeyFrame* MapPoint::GetReferenceKeyFrame()
{
	unique_lock<mutex> lock(m_MutexFeatures);
	return m_RefKF;
}


std::map<KeyFrame*,size_t> MapPoint::GetObservations()
{
	 unique_lock<mutex> lock(m_MutexFeatures);
    return m_Observations;
}

int MapPoint::Observations()
{
	unique_lock<mutex> lock(m_MutexFeatures);
    return m_ObsNumber;
}


void MapPoint::AddObservation(KeyFrame* pKF,size_t idx)
{
	unique_lock<mutex> lock(m_MutexFeatures);
	if(m_Observations.count(pKF))
	{
		return;
	}

	m_Observations[pKF]=idx;

	if(pKF->m_vectorURight[idx]>=0)
	{
		m_ObsNumber += 2;
	}
	else
	{
		m_ObsNumber++;
	}
}

void MapPoint::EraseObservation(KeyFrame* pKF)
{
	bool bBad=false;

	{
		unique_lock<mutex> lock(m_MutexFeatures);
		if(m_Observations.count(pKF))
		{
			int idx = m_Observations[pKF];
			if(pKF->m_vectorURight[idx]>=0)
			{
			 	m_ObsNumber -= 2;
			}
			else
			{
				m_ObsNumber--;
			}

			m_Observations.erase(pKF);

			if(m_RefKF == pKF)
			{
			 	m_RefKF = m_Observations.begin()->first;
			}

			// If only 2 observations or less, discard point
			if(m_ObsNumber <= 2)
			{
				bBad=true;
			}
		}
	}

	if(bBad)
	{
		SetBadMapPointFlag();
	}
}


int MapPoint::GetIndexInKeyFrame(KeyFrame* pKF)
{
	unique_lock<mutex> lock(m_MutexFeatures);
	if(m_Observations.count(pKF))
	{
		return m_Observations[pKF];
	}
	else
	{
		return -1;
	}
}

bool MapPoint::IsInKeyFrame(KeyFrame* pKF)
{
	unique_lock<mutex> lock(m_MutexFeatures);
	return (m_Observations.count(pKF));
}



void MapPoint::SetBadMapPointFlag()
{
	map<KeyFrame*,size_t> obs;

	{
		unique_lock<mutex> lock1(m_MutexFeatures);
		unique_lock<mutex> lock2(m_MutexPos);
		m_BadMapPoint = true;

		obs = m_Observations;
		m_Observations.clear();
	}

	for(map<KeyFrame*,size_t>::iterator it=obs.begin(), end=obs.end(); it!=end; it++)
    {
        KeyFrame* pKF = it->first;
        pKF->EraseMapPointMatch(it->second);
    }

	m_Map->EraseMapPoint(this);
}

bool MapPoint::isBadMapPoint()
{
	unique_lock<mutex> lock(m_MutexFeatures);
	unique_lock<mutex> lock2(m_MutexPos);
	return m_BadMapPoint;
}

void MapPoint::Replace(MapPoint* pMP)
{
	if(pMP->m_MapPointId == this->m_MapPointId)
	{
		return;
	}

	int visible, found;
	map<KeyFrame*,size_t> obs;

	{
		unique_lock<mutex> lock1(m_MutexFeatures);
		unique_lock<mutex> lock2(m_MutexPos);
		obs=m_Observations;
		m_Observations.clear();
		m_BadMapPoint = true;
		visible = m_VisibleCount;
		found = m_FoundCount;
		m_Replaced = pMP;
	}

	for(map<KeyFrame*,size_t>::iterator it=obs.begin(), end=obs.end(); it!=end; it++)
	{
		// Replace measurement in keyframe
		KeyFrame* pKF = it->first;

		if(!pMP->IsInKeyFrame(pKF))
		{
			pKF->ReplaceMapPointMatch(it->second, pMP);
			pMP->AddObservation(pKF,it->second);
		}
		else
		{
			pKF->EraseMapPointMatch(it->second);
		}
	}

	pMP->IncreaseFound(found);
	pMP->IncreaseVisible(visible);
	pMP->ComputeDistinctiveDescriptors();

	m_Map->EraseMapPoint(this);
}

MapPoint* MapPoint::GetReplaced()
{
	unique_lock<mutex> lock1(m_MutexFeatures);
	unique_lock<mutex> lock2(m_MutexPos);
	return m_Replaced;
}


void MapPoint::IncreaseVisible(int n)
{
	unique_lock<mutex> lock(m_MutexFeatures);
    m_VisibleCount += n;
}

void MapPoint::IncreaseFound(int n)
{
	unique_lock<mutex> lock(m_MutexFeatures);
    m_FoundCount += n;
}

void MapPoint::ComputeDistinctiveDescriptors()
{
	// Retrieve all observed descriptors
	vector<cv::Mat> vDescriptors;

	map<KeyFrame*,size_t> observations;

	{
		 unique_lock<mutex> lock1(m_MutexFeatures);
		 if(m_BadMapPoint)
		 {
			return;
		 }

		 observations=m_Observations;
	}

	if(observations.empty())
	{
		return;
	}

	vDescriptors.reserve(observations.size());

	for(map<KeyFrame*,size_t>::iterator it=observations.begin(), end=observations.end(); it!=end; it++)
	{
		 KeyFrame* pKF = it->first;

		if(!pKF->isBadKeyFrame())
		{
			vDescriptors.push_back(pKF->m_Descriptors.row(it->second));
		}
	}


	if(vDescriptors.empty())
	{
		return;
	}

	// Compute distances between them
	const size_t n = vDescriptors.size();
	float Distances[n][n];

	for(size_t i=0; i<n; i++)
	{
		Distances[i][i]=0;
		for(size_t j=i+1; j<n; j++)
		{
			int distij = cv::norm(vDescriptors[i],vDescriptors[j]);
			 Distances[i][j] = distij;
			 Distances[j][i] = distij;
		}
	}

	// Take the descriptor with least median distance to the rest
	int BestMedian = INT_MAX;
    int BestIdx = 0;

	for(size_t i=0; i<n; i++)
	{
		vector<int> vDists(Distances[i], Distances[i]+n);
		sort(vDists.begin(), vDists.end());
		int median = vDists[0.5 * (n-1)];

		if(median < BestMedian)
		{
			BestMedian = median;
			BestIdx = i;
		}
	}

	{
		unique_lock<mutex> lock(m_MutexFeatures);
		m_MapPointDescriptor = vDescriptors[BestIdx].clone();
	}
}


void MapPoint::UpdateNormalAndDepth()
{
	map<KeyFrame*,size_t> observations;
	KeyFrame* pRefKF;
	cv::Mat Pos;

	{
		unique_lock<mutex> lock1(m_MutexFeatures);
		unique_lock<mutex> lock2(m_MutexPos);

		if(m_BadMapPoint)
		{
			return;
		}

		observations = m_Observations;
		pRefKF = m_RefKF;
		Pos = m_WorldPos.clone();
	}

	if(observations.empty())
	{
		return;
	}

	cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
	int n=0;
	for(map<KeyFrame*,size_t>::iterator it=observations.begin(), end=observations.end(); it!=end; it++)
	{
		KeyFrame* pKF = it->first;
		cv::Mat Owi = pKF->GetCameraCenter();
		cv::Mat normali = m_WorldPos - Owi;
		normal = normal + normali/cv::norm(normali);
		n++;
	}

	cv::Mat PC = Pos - pRefKF->GetCameraCenter();
	

	{
		unique_lock<mutex> lock3(m_MutexPos);
		m_NormalVector = normal/n;
		m_scaleInvDistance = cv::norm(PC);
	}
}


cv::Mat MapPoint::GetDescriptor()
{
	unique_lock<mutex> lock(m_MutexFeatures);
	return m_MapPointDescriptor.clone();
}


float MapPoint::GetFoundRatio()
{
	unique_lock<mutex> lock(m_MutexFeatures);
    return static_cast<float>(m_FoundCount) / m_VisibleCount;
}

